
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_wpnav.c
  * @author     baiyang
  * @date       2022-3-10
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mc_wpnav.h"

#include <stdio.h>
#include <string.h>

#include <rtconfig.h>
#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/
// maximum velocities and accelerations
#define WPNAV_ACCELERATION              250.0f      // maximum horizontal acceleration in cm/s/s that wp navigation will request
#define WPNAV_WP_SPEED                 1000.0f      // default horizontal speed between waypoints in cm/s
#define WPNAV_WP_SPEED_MIN               20.0f      // minimum horizontal speed between waypoints in cm/s
#define WPNAV_WP_RADIUS                 200.0f      // default waypoint radius in cm
#define WPNAV_WP_RADIUS_MIN               5.0f      // minimum waypoint radius in cm
#define WPNAV_WP_SPEED_UP               250.0f      // default maximum climb velocity
#define WPNAV_WP_SPEED_DOWN             150.0f      // default maximum descent velocity
#define WPNAV_WP_ACCEL_Z_DEFAULT        100.0f      // default vertical acceleration between waypoints in cm/s/s
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void assign_param(mc_wpnav* wpnav);
static void wpnav_param_set_and_save_ifchanged(const char* param_name, double value);
static void wpnav_calc_scurve_jerk_and_snap(mc_wpnav* wpnav);
/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    PARAM_DEFINE_FLOAT(WPNAV_SPEED, 1000.0f),
    PARAM_DEFINE_FLOAT(WPNAV_RADIUS, 200.0f),
    PARAM_DEFINE_FLOAT(WPNAV_SPEED_UP, 250.0f),
    PARAM_DEFINE_FLOAT(WPNAV_SPEED_DN, 150.0f),
    PARAM_DEFINE_FLOAT(WPNAV_ACCEL, 250.0f),
    PARAM_DEFINE_FLOAT(WPNAV_ACCEL_Z, 100.0f),
    PARAM_DEFINE_INT8(WPNAV_RFND_USE, 1),
    PARAM_DEFINE_FLOAT(WPNAV_JERK, 1.0f),
    PARAM_DEFINE_FLOAT(WPNAV_TER_MARGIN, 10.0),
};
PARAM_GROUP_DEFINE(WPNAV, var_info);
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   wpnav  
  * @param[in]   ahrs  
  * @param[in]   pos_control  
  * @param[out]  
  * @retval      
  * @note        
  */
void wpnav_ctor(mc_wpnav* wpnav, ahrs_view* ahrs, Attitude_ctrl* attitude_control, Position_ctrl* pos_control)
{
    assign_param(wpnav);

    wpnav->_ahrs             = ahrs;
    wpnav->_attitude_control = attitude_control;
    wpnav->_pos_control      = pos_control;

    wpnav->_rangefinder_healthy = false;

    // init flags
    wpnav->_flags.reached_destination = false;
    wpnav->_flags.fast_waypoint = false;

    // initialise old WPNAV_SPEED values
    wpnav->_last_wp_speed_cms = wpnav->_wp_speed_cms;
    wpnav->_last_wp_speed_up_cms = wpnav->_wp_speed_up_cms;
    wpnav->_last_wp_speed_down_cms = wpnav_get_default_speed_down(wpnav);

    scurve_ctor(&wpnav->_scurve_prev_leg);
    scurve_ctor(&wpnav->_scurve_this_leg);
    scurve_ctor(&wpnav->_scurve_next_leg);

    slcurve_ctor(&wpnav->_spline_this_leg);
    slcurve_ctor(&wpnav->_spline_next_leg);
}

// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
enum WPNavTerrainSource wpnav_get_terrain_source(mc_wpnav* wpnav)
{
    // use range finder if connected
    if (wpnav->_rangefinder_available && wpnav->_rangefinder_use) {
        return WPNAV_TERRAIN_FROM_RANGEFINDER;
    }

    return WPNAV_TERRAIN_UNAVAILABLE;
}

// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool wpnav_get_terrain_offset(mc_wpnav* wpnav, float* offset_cm)
{
    // calculate offset based on source (rangefinder or terrain database)
    switch (wpnav_get_terrain_source(wpnav)) {
    case WPNAV_TERRAIN_UNAVAILABLE:
        return false;
    case WPNAV_TERRAIN_FROM_RANGEFINDER:
        if (wpnav->_rangefinder_healthy) {
            *offset_cm = wpnav->_ahrs->relpos_cm.z - wpnav->_rangefinder_alt_cm;
            return true;
        }
        return false;
    case WPNAV_TERRAIN_FROM_TERRAINDATABASE: 
        return false;
    }

    // we should never get here
    return false;
}

// convert location to vector from ekf origin.  terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
//      returns false if conversion failed (likely because terrain data was not available)
bool wpnav_get_vector_NEU(const Location *loc, Vector3f_t *vec, bool *terrain_alt)
{
    // convert location to NE vector2f
    Vector2f_t res_vec;
    if (!location_get_vector_xy_from_origin_ne(loc, &res_vec)) {
        return false;
    }

    // convert altitude
    if (location_get_alt_frame(loc) == ALT_FRAME_ABOVE_TERRAIN) {
        int32_t terr_alt;
        if (!location_get_alt_cm(loc, ALT_FRAME_ABOVE_TERRAIN, &terr_alt)) {
            return false;
        }
        vec->z = terr_alt;
        *terrain_alt = true;
    } else {
        *terrain_alt = false;
        int32_t temp_alt;
        if (!location_get_alt_cm(loc, ALT_FRAME_ABOVE_ORIGIN, &temp_alt)) {
            return false;
        }
        vec->z = temp_alt;
        *terrain_alt = false;
    }

    // copy xy (we do this to ensure we do not adjust vector unless the overall conversion is successful
    vec->x = res_vec.x;
    vec->y = res_vec.y;

    return true;
}

///
/// waypoint navigation
///

/// wp_and_spline_init - initialise straight line and spline waypoint controllers
///     speed_cms should be a positive value or left at zero to use the default speed
///     stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero
///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
void wpnav_wp_and_spline_init(mc_wpnav* wpnav, float speed_cms, Vector3f_t stopping_point)
{
    // sanity check parameters
    // check _wp_accel_cmss is reasonable
    wpnav->_scurve_accel_corner = control_angle_to_accel(posctrl_get_lean_angle_max_cd(wpnav->_pos_control) * 0.01f) * 100;
    const float wp_accel_cmss = MIN(wpnav->_wp_accel_cmss, wpnav->_scurve_accel_corner);
    wpnav_param_set_and_save_ifchanged("WPNAV_ACCEL", (wpnav->_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss);

    // check _wp_radius_cm is reasonable
    wpnav_param_set_and_save_ifchanged("WPNAV_RADIUS", MAX(wpnav->_wp_radius_cm, WPNAV_WP_RADIUS_MIN));

    // check _wp_speed
    wpnav_param_set_and_save_ifchanged("WPNAV_SPEED", MAX(wpnav->_wp_speed_cms, WPNAV_WP_SPEED_MIN));

    // initialise position controller
    posctrl_init_z_controller_stopping_point(wpnav->_pos_control);
    posctrl_init_xy_controller_stopping_point(wpnav->_pos_control);

    // initialize the desired wp speed if not already done
    wpnav->_wp_desired_speed_xy_cms = math_flt_positive(speed_cms) ? speed_cms : wpnav->_wp_speed_cms;
    wpnav->_wp_desired_speed_xy_cms = MAX(wpnav->_wp_desired_speed_xy_cms, WPNAV_WP_SPEED_MIN);

    // initialise position controller speed and acceleration
    posctrl_set_max_speed_accel_xy(wpnav->_pos_control, wpnav->_wp_desired_speed_xy_cms, wpnav->_wp_accel_cmss);
    posctrl_set_correction_speed_accel_xy(wpnav->_pos_control, wpnav->_wp_desired_speed_xy_cms, wpnav->_wp_accel_cmss);
    posctrl_set_max_speed_accel_z(wpnav->_pos_control, -wpnav_get_default_speed_down(wpnav), wpnav->_wp_speed_up_cms, wpnav->_wp_accel_z_cmss);
    posctrl_set_correction_speed_accel_z(wpnav->_pos_control, -wpnav_get_default_speed_down(wpnav), wpnav->_wp_speed_up_cms, wpnav->_wp_accel_z_cmss);

    // calculate scurve jerk and jerk time
    if (!math_flt_positive(wpnav->_wp_jerk)) {
        wpnav->_wp_jerk = wpnav->_wp_accel_cmss;
    }
    wpnav_calc_scurve_jerk_and_snap(wpnav);

    scurve_init(&wpnav->_scurve_prev_leg);
    scurve_init(&wpnav->_scurve_this_leg);
    scurve_init(&wpnav->_scurve_next_leg);
    wpnav->_track_scalar_dt = 1.0f;

    wpnav->_flags.reached_destination = true;
    wpnav->_flags.fast_waypoint = false;

    // initialise origin and destination to stopping point
    if (vec3_is_zero(&stopping_point)) {
        wpnav_get_wp_stopping_point(wpnav, &stopping_point);
    }
    wpnav->_origin = wpnav->_destination = stopping_point;
    wpnav->_terrain_alt = false;
    wpnav->_this_leg_is_spline = false;

    // initialise the terrain velocity to the current maximum velocity
    wpnav->_offset_vel = wpnav->_wp_desired_speed_xy_cms;
    wpnav->_offset_accel = 0.0;
    wpnav->_paused = false;

    // mark as active
    wpnav->_wp_last_update = time_millis();
}

/// wp_and_spline_init2 - initialise straight line and spline waypoint controllers
///     speed_cms should be a positive value or left at zero to use the default speed
///     stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero
///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
void wpnav_wp_and_spline_init2(mc_wpnav* wpnav)
{
    Vector3f_t v3_zero = VECTOR3F_DEFAULT_VALUE;

    wpnav_wp_and_spline_init(wpnav, 0.0f, v3_zero);
}

/// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
void wpnav_set_speed_xy(mc_wpnav* wpnav, float speed_cms)
{
    // range check target speed and protect against divide by zero
    if (speed_cms >= WPNAV_WP_SPEED_MIN && math_flt_positive(wpnav->_wp_desired_speed_xy_cms)) {
        // update horizontal velocity speed offset scalar
        wpnav->_offset_vel = speed_cms * wpnav->_offset_vel / wpnav->_wp_desired_speed_xy_cms;

        // initialize the desired wp speed
        wpnav->_wp_desired_speed_xy_cms = speed_cms;

        // update position controller speed and acceleration
        posctrl_set_max_speed_accel_xy(wpnav->_pos_control, wpnav->_wp_desired_speed_xy_cms, wpnav->_wp_accel_cmss);
        posctrl_set_correction_speed_accel_xy(wpnav->_pos_control, wpnav->_wp_desired_speed_xy_cms, wpnav->_wp_accel_cmss);

        // change track speed
        wpnav_update_track_with_speed_accel_limits(wpnav);
    }
}

/// set current target climb rate during wp navigation
void wpnav_set_speed_up(mc_wpnav* wpnav, float speed_up_cms)
{
    posctrl_set_max_speed_accel_z(wpnav->_pos_control, posctrl_get_max_speed_down_cms(wpnav->_pos_control), speed_up_cms, posctrl_get_max_accel_z_cmss(wpnav->_pos_control));
    wpnav_update_track_with_speed_accel_limits(wpnav);
}

/// set current target descent rate during wp navigation
void wpnav_set_speed_down(mc_wpnav* wpnav, float speed_down_cms)
{
    posctrl_set_max_speed_accel_z(wpnav->_pos_control, speed_down_cms, posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_accel_z_cmss(wpnav->_pos_control));
    wpnav_update_track_with_speed_accel_limits(wpnav);
}

/// recalculate path with update speed and/or acceleration limits
void wpnav_update_track_with_speed_accel_limits(mc_wpnav* wpnav)
{
    // update this leg
    if (wpnav->_this_leg_is_spline) {
        slcurve_set_speed_accel(&wpnav->_spline_this_leg, posctrl_get_max_speed_xy_cms(wpnav->_pos_control), posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_speed_down_cms(wpnav->_pos_control),
                                         wpnav->_wp_accel_cmss, wpnav->_wp_accel_z_cmss);
    } else {
        scurve_set_speed_max(&wpnav->_scurve_this_leg , posctrl_get_max_speed_xy_cms(wpnav->_pos_control), posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_speed_down_cms(wpnav->_pos_control));
    }

    // update next leg
    if (wpnav->_next_leg_is_spline) {
        slcurve_set_speed_accel(&wpnav->_spline_next_leg, posctrl_get_max_speed_xy_cms(wpnav->_pos_control), posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_speed_down_cms(wpnav->_pos_control),
                                         wpnav->_wp_accel_cmss, wpnav->_wp_accel_z_cmss);
    } else {
        scurve_set_speed_max(&wpnav->_scurve_next_leg, posctrl_get_max_speed_xy_cms(wpnav->_pos_control), posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_speed_down_cms(wpnav->_pos_control));
    }
}

/// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void wpnav_get_wp_stopping_point_xy(mc_wpnav* wpnav, Vector2f_t* stopping_point)
{
    Vector2p stop;
    posctrl_get_stopping_point_xy_cm(wpnav->_pos_control, &stop);

    stopping_point->x = stop.x;
    stopping_point->y = stop.y;
}

/// get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity
void wpnav_get_wp_stopping_point(mc_wpnav* wpnav, Vector3f_t* stopping_point)
{
    Vector3p stop;
    posctrl_get_stopping_point_xy_cm(wpnav->_pos_control, (Vector2p*)&stop);
    posctrl_get_stopping_point_z_cm(wpnav->_pos_control, &stop.z);

    stopping_point->x = stop.x;
    stopping_point->y = stop.y;
    stopping_point->z = stop.z;
}

/// set_wp_destination waypoint using location class
///     returns false if conversion from location to vector from ekf origin cannot be calculated
bool wpnav_set_wp_destination_loc(mc_wpnav* wpnav, const Location* destination)
{
    bool terr_alt;
    Vector3f_t dest_neu = VECTOR3F_DEFAULT_VALUE;

    // convert destination location to vector
    if (!wpnav_get_vector_NEU(destination, &dest_neu, &terr_alt)) {
        return false;
    }

    // set target as vector from EKF origin
    return wpnav_set_wp_destination(wpnav, &dest_neu, terr_alt);
}

/// set next destination using location class
///     returns false if conversion from location to vector from ekf origin cannot be calculated
bool wpnav_set_wp_destination_next_loc(mc_wpnav* wpnav, const Location* destination)
{
    bool terr_alt;
    Vector3f_t dest_neu = VECTOR3F_DEFAULT_VALUE;

    // convert destination location to vector
    if (!wpnav_get_vector_NEU(destination, &dest_neu, &terr_alt)) {
        return false;
    }

    // set target as vector from EKF origin
    return wpnav_set_wp_destination_next(wpnav, &dest_neu, terr_alt);
}

// get destination as a location. Altitude frame will be above origin or above terrain
// returns false if unable to return a destination (for example if origin has not yet been set)
bool wpnav_get_wp_destination_loc(mc_wpnav* wpnav, Location* destination)
{
    if (!ahrs_get_origin(wpnav->_ahrs, destination)) {
        return false;
    }

    Location_from_vector_xy_alt(destination, wpnav_get_wp_destination(wpnav), wpnav->_terrain_alt ? ALT_FRAME_ABOVE_TERRAIN : ALT_FRAME_ABOVE_ORIGIN);
    return true;
}

/// set_wp_destination - set destination waypoints using position vectors (distance from ekf origin in cm)
///     terrain_alt should be true if destination.z is an altitude above terrain (false if alt-above-ekf-origin)
///     returns false on failure (likely caused by missing terrain data)
bool wpnav_set_wp_destination(mc_wpnav* wpnav, const Vector3f_t* destination, bool terrain_alt)
{
    Vector3f_t v3_zero = VECTOR3F_DEFAULT_VALUE;

    // re-initialise if previous destination has been interrupted
    if (!wpnav_is_active(wpnav) || !wpnav->_flags.reached_destination) {
        wpnav_wp_and_spline_init(wpnav, wpnav->_wp_desired_speed_xy_cms, v3_zero);
    }

    scurve_init(&wpnav->_scurve_prev_leg);
    float origin_speed = 0.0f;

    // use previous destination as origin
    wpnav->_origin = wpnav->_destination;

    if (terrain_alt == wpnav->_terrain_alt) {
        if (wpnav->_this_leg_is_spline) {
            // if previous leg was a spline we can use current target velocity vector for origin velocity vector
            Vector3f_t curr_target_vel = posctrl_get_vel_desired_cms(wpnav->_pos_control);
            curr_target_vel.z -= posctrl_get_vel_offset_z_cms(wpnav->_pos_control);
            origin_speed = vec3_length(&curr_target_vel);
        } else {
            // store previous leg
            wpnav->_scurve_prev_leg = wpnav->_scurve_this_leg;
        }
    } else {

        // get current alt above terrain
        float origin_terr_offset;
        if (!wpnav_get_terrain_offset(wpnav, &origin_terr_offset)) {
            return false;
        }

        // convert origin to alt-above-terrain if necessary
        if (terrain_alt) {
            // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
            wpnav->_origin.z -= origin_terr_offset;
            posctrl_set_pos_offset_z_cm(wpnav->_pos_control, posctrl_get_pos_offset_z_cm(wpnav->_pos_control) + origin_terr_offset);
        } else {
            // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
            wpnav->_origin.z += origin_terr_offset;
            posctrl_set_pos_offset_z_cm(wpnav->_pos_control, posctrl_get_pos_offset_z_cm(wpnav->_pos_control) - origin_terr_offset);
        }
    }

    // update destination
    wpnav->_destination = *destination;
    wpnav->_terrain_alt = terrain_alt;

    if (wpnav->_flags.fast_waypoint && !wpnav->_this_leg_is_spline && !wpnav->_next_leg_is_spline && !scurve_finished(&wpnav->_scurve_next_leg)) {
        wpnav->_scurve_this_leg = wpnav->_scurve_next_leg;
    } else {
        scurve_calculate_track(&wpnav->_scurve_this_leg, &wpnav->_origin, &wpnav->_destination,
                                         posctrl_get_max_speed_xy_cms(wpnav->_pos_control), posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_speed_down_cms(wpnav->_pos_control),
                                         wpnav->_wp_accel_cmss, wpnav->_wp_accel_z_cmss,
                                         wpnav->_scurve_snap * 100.0f, wpnav->_scurve_jerk * 100.0f);
        if (!math_flt_zero(origin_speed)) {
            // rebuild start of scurve if we have a non-zero origin speed
            scurve_set_origin_speed_max(&wpnav->_scurve_this_leg, origin_speed);
        }
    }

    wpnav->_this_leg_is_spline = false;
    scurve_init(&wpnav->_scurve_next_leg);
    wpnav->_flags.fast_waypoint = false;   // default waypoint back to slow
    wpnav->_flags.reached_destination = false;

    return true;
}

/// set next destination using position vector (distance from ekf origin in cm)
///     terrain_alt should be true if destination.z is a desired altitude above terrain
///     provide next_destination
bool wpnav_set_wp_destination_next(mc_wpnav* wpnav, const Vector3f_t* destination, bool terrain_alt)
{
    // do not add next point if alt types don't match
    if (terrain_alt != wpnav->_terrain_alt) {
        return true;
    }

    scurve_calculate_track(&wpnav->_scurve_next_leg, &wpnav->_destination, destination,
                                     posctrl_get_max_speed_xy_cms(wpnav->_pos_control), posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_speed_down_cms(wpnav->_pos_control),
                                     wpnav->_wp_accel_cmss, wpnav->_wp_accel_z_cmss,
                                     wpnav->_scurve_snap * 100.0f, wpnav->_scurve_jerk * 100.0f);
    if (wpnav->_this_leg_is_spline) {
        const float this_leg_dest_speed_max = slcurve_get_destination_speed_max(&wpnav->_spline_this_leg);
        const float next_leg_origin_speed_max = scurve_set_origin_speed_max(&wpnav->_scurve_next_leg, this_leg_dest_speed_max);
        slcurve_set_destination_speed_max(&wpnav->_spline_this_leg, next_leg_origin_speed_max);
    }
    wpnav->_next_leg_is_spline = false;

    // next destination provided so fast waypoint
    wpnav->_flags.fast_waypoint = true;

    return true;
}

/// set waypoint destination using NED position vector from ekf origin in meters
bool wpnav_set_wp_destination_NED(mc_wpnav* wpnav, const Vector3f_t* destination_NED)
{
    Vector3f_t destination_NEU = {destination_NED->x * 100.0f, destination_NED->y * 100.0f, -destination_NED->z * 100.0f};

    // convert NED to NEU and do not use terrain following
    return wpnav_set_wp_destination(wpnav, &destination_NEU, false);
}

/// set waypoint destination using NED position vector from ekf origin in meters
bool wpnav_set_wp_destination_next_NED(mc_wpnav* wpnav, const Vector3f_t* destination_NED)
{
    Vector3f_t destination_NEU = {destination_NED->x * 100.0f, destination_NED->y * 100.0f, -destination_NED->z * 100.0f};

    // convert NED to NEU and do not use terrain following
    return wpnav_set_wp_destination_next(wpnav, &destination_NEU, false);
}

/// shifts the origin and destination horizontally to the current position
///     used to reset the track when taking off without horizontal position control
///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
void wpnav_shift_wp_origin_and_destination_to_current_pos_xy(mc_wpnav* wpnav)
{
    // Reset position controller to current location
    posctrl_init_xy_controller(wpnav->_pos_control);

    // shift origin and destination horizontally
    wpnav->_origin.x = wpnav->_ahrs->relpos_cm.x;
    wpnav->_origin.y = wpnav->_ahrs->relpos_cm.y;

    wpnav->_destination.x = wpnav->_ahrs->relpos_cm.x;
    wpnav->_destination.y = wpnav->_ahrs->relpos_cm.y;
}

/// shifts the origin and destination horizontally to the achievable stopping point
///     used to reset the track when horizontal navigation is enabled after having been disabled (see Copter's wp_navalt_min)
///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
void wpnav_shift_wp_origin_and_destination_to_stopping_point_xy(mc_wpnav* wpnav)
{
    // relax position control in xy axis
    // removing velocity error also impacts stopping point calculation
    posctrl_relax_velocity_controller_xy(wpnav->_pos_control);

    // get current and target locations
    Vector2f_t stopping_point;
    wpnav_get_wp_stopping_point_xy(wpnav, &stopping_point);

    // shift origin and destination horizontally
    wpnav->_origin.x = stopping_point.x;
    wpnav->_origin.y = stopping_point.y;

    wpnav->_destination.x = stopping_point.x;
    wpnav->_destination.y = stopping_point.y;

    // move pos controller target horizontally
    posctrl_set_pos_target_xy_cm(wpnav->_pos_control, stopping_point.x, stopping_point.y);
}

/// advance_wp_target_along_track - move target location along track from origin to destination
bool wpnav_advance_wp_target_along_track(mc_wpnav* wpnav, float dt)
{
    // calculate terrain adjustments
    float terr_offset = 0.0f;
    if (wpnav->_terrain_alt && !wpnav_get_terrain_offset(wpnav, &terr_offset)) {
        return false;
    }
    const float offset_z_scaler = posctrl_pos_offset_z_scaler(wpnav->_pos_control, terr_offset, wpnav_get_terrain_margin(wpnav) * 100.0f);

    // input shape the terrain offset
    posctrl_update_pos_offset_z(wpnav->_pos_control, terr_offset);

    // get current position and adjust altitude to origin and destination's frame (i.e. _frame)
    const Vector3f_t curr_pos = {wpnav->_ahrs->relpos_cm.x, wpnav->_ahrs->relpos_cm.y, wpnav->_ahrs->relpos_cm.z - terr_offset};
    Vector3f_t curr_target_vel = posctrl_get_vel_desired_cms(wpnav->_pos_control);
    curr_target_vel.z -= posctrl_get_vel_offset_z_cms(wpnav->_pos_control);

    // Use _track_scalar_dt to slow down progression of the position target moving too far in front of aircraft
    // _track_scalar_dt does not scale the velocity or acceleration
    float track_scaler_dt = 1.0f;
    // check target velocity is non-zero
    if (math_flt_positive(vec3_length_squared(&curr_target_vel))) {
        Vector3f_t track_direction = VECTOR3F_DEFAULT_VALUE;
        vec3_norm(&track_direction, &curr_target_vel);

        Vector3f_t pos_error_cm = posctrl_get_pos_error_cm(wpnav->_pos_control);
        const float track_error = vec3_dot(&pos_error_cm, &track_direction);
        const float track_velocity = vec3_dot(&wpnav->_ahrs->velocity_cm, &track_direction);
        // set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation.
        track_scaler_dt = math_constrain_float(0.05f + (track_velocity - posctrl_get_pos_xy_p(wpnav->_pos_control)->_kp * track_error) / vec3_length(&curr_target_vel), 0.1f, 1.0f);
    }

    // Use vel_scaler_dt to slow down the trajectory time
    // vel_scaler_dt scales the velocity and acceleration to be kinematically constent
    float vel_scaler_dt = 1.0;
    if (math_flt_positive(wpnav->_wp_desired_speed_xy_cms)) {
        control_update_vel_accel(&wpnav->_offset_vel, wpnav->_offset_accel, dt, 0.0f, 0.0f);
        const float vel_input = !wpnav->_paused ? wpnav->_wp_desired_speed_xy_cms * offset_z_scaler : 0.0;
        control_shape_vel_accel(vel_input, 0.0f, wpnav->_offset_vel, &wpnav->_offset_accel, -wpnav->_wp_accel_cmss, wpnav->_wp_accel_cmss,
                        posctrl_get_shaping_jerk_xy_cmsss(wpnav->_pos_control), dt, true);
        vel_scaler_dt = wpnav->_offset_vel / wpnav->_wp_desired_speed_xy_cms;
    }

    // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
    float track_scaler_tc = 1.0f;
    if (!math_flt_zero(wpnav->_wp_jerk)) {
        track_scaler_tc = 0.01f * wpnav->_wp_accel_cmss/wpnav->_wp_jerk;
    }
    wpnav->_track_scalar_dt += (track_scaler_dt - wpnav->_track_scalar_dt) * (dt / track_scaler_tc);

    // target position, velocity and acceleration from straight line or spline calculators
    Vector3f_t target_pos = VECTOR3F_DEFAULT_VALUE, target_vel = VECTOR3F_DEFAULT_VALUE, target_accel = VECTOR3F_DEFAULT_VALUE;

    bool s_finished;
    if (!wpnav->_this_leg_is_spline) {
        // update target position, velocity and acceleration
        target_pos = wpnav->_origin;
        s_finished = scurve_advance_target_along_track(&wpnav->_scurve_this_leg, &wpnav->_scurve_prev_leg, &wpnav->_scurve_next_leg, wpnav->_wp_radius_cm, wpnav->_scurve_accel_corner, wpnav->_flags.fast_waypoint, wpnav->_track_scalar_dt * vel_scaler_dt * dt, &target_pos, &target_vel, &target_accel);
    } else {
        // splinetarget_vel
        target_vel = curr_target_vel;
        slcurve_advance_target_along_track(&wpnav->_spline_this_leg, wpnav->_track_scalar_dt * vel_scaler_dt * dt, &target_pos, &target_vel);
        s_finished = slcurve_reached_destination(&wpnav->_spline_this_leg);
    }

    Vector3f_t accel_offset = VECTOR3F_DEFAULT_VALUE;
    if (math_flt_positive(vec3_length_squared(&target_vel))) {
        Vector3f_t track_direction = VECTOR3F_DEFAULT_VALUE;
        vec3_norm(&track_direction, &target_vel);
        vec3_mult(&accel_offset, &track_direction, wpnav->_offset_accel * vec3_length(&target_vel) / wpnav->_wp_desired_speed_xy_cms);
    }

    vec3_mult(&target_vel, &target_vel, vel_scaler_dt);
    vec3_mult(&target_accel, &target_accel, sq(vel_scaler_dt));
    vec3_add(&target_accel, &target_accel, &accel_offset);

    // convert final_target.z to altitude above the ekf origin
    target_pos.z += posctrl_get_pos_offset_z_cm(wpnav->_pos_control);
    target_vel.z += posctrl_get_vel_offset_z_cms(wpnav->_pos_control);
    target_accel.z += posctrl_get_accel_offset_z_cmss(wpnav->_pos_control);

    // pass new target to the position controller
    posctrl_set_pos_vel_accel(wpnav->_pos_control, &target_pos, &target_vel, &target_accel);

    // check if we've reached the waypoint
    if (!wpnav->_flags.reached_destination) {
        if (s_finished) {
            // "fast" waypoints are complete once the intermediate point reaches the destination
            if (wpnav->_flags.fast_waypoint) {
                wpnav->_flags.reached_destination = true;
            } else {
                // regular waypoints also require the copter to be within the waypoint radius
                const Vector3f_t dist_to_dest = {curr_pos.x - wpnav->_destination.x,
                                                 curr_pos.y - wpnav->_destination.y,
                                                 curr_pos.z - wpnav->_destination.z};
                if (vec3_length_squared(&dist_to_dest) <= sq(wpnav->_wp_radius_cm)) {
                    wpnav->_flags.reached_destination = true;
                }
            }
        }
    }

    // successfully advanced along track
    return true;
}

/// get_wp_distance_to_destination - get horizontal distance to destination in cm
float wpnav_get_wp_distance_to_destination(mc_wpnav* wpnav)
{
    return vec3_get_horizontal_distance_cm(&wpnav->_ahrs->relpos_cm, &wpnav->_destination);
}

/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t wpnav_get_wp_bearing_to_destination(mc_wpnav* wpnav)
{
    return vec3_get_bearing_cd(&wpnav->_ahrs->relpos_cm, &wpnav->_destination);
}

/// update_wpnav - run the wp controller - should be called at 100hz or higher
bool wpnav_update_wpnav(mc_wpnav* wpnav)
{
    bool ret = true;

    if (!math_flt_equal(wpnav->_wp_speed_cms, wpnav->_last_wp_speed_cms)) {
        wpnav_set_speed_xy(wpnav, wpnav->_wp_speed_cms);
        wpnav->_last_wp_speed_cms = wpnav->_wp_speed_cms;
    }
    if (!math_flt_equal(wpnav->_wp_speed_up_cms, wpnav->_last_wp_speed_up_cms)) {
        wpnav_set_speed_up(wpnav, wpnav->_wp_speed_up_cms);
        wpnav->_last_wp_speed_up_cms = wpnav->_wp_speed_up_cms;
    }
    if (!math_flt_equal(wpnav->_wp_speed_down_cms, wpnav->_last_wp_speed_down_cms)) {
        wpnav_set_speed_down(wpnav, wpnav->_wp_speed_down_cms);
        wpnav->_last_wp_speed_down_cms = wpnav->_wp_speed_down_cms;
    }

    // advance the target if necessary
    if (!wpnav_advance_wp_target_along_track(wpnav, posctrl_get_dt(wpnav->_pos_control))) {
        // To-Do: handle inability to advance along track (probably because of missing terrain data)
        ret = false;
    }

    posctrl_update_xy_controller(wpnav->_pos_control);

    wpnav->_wp_last_update = time_millis();

    return ret;
}

///
/// spline methods
///

/// set_spline_destination waypoint using location class
///     returns false if conversion from location to vector from ekf origin cannot be calculated
///     next_destination should be the next segment's destination
///     next_is_spline should be true if path to next_destination should be a spline
bool wpnav_set_spline_destination_loc(mc_wpnav* wpnav, const Location* destination, const Location* next_destination, bool next_is_spline)
{
    // convert destination location to vector
    Vector3f_t dest_neu = VECTOR3F_DEFAULT_VALUE;
    bool dest_terr_alt;
    if (!wpnav_get_vector_NEU(destination, &dest_neu, &dest_terr_alt)) {
        return false;
    }

    // convert next destination to vector
    Vector3f_t next_dest_neu = VECTOR3F_DEFAULT_VALUE;
    bool next_dest_terr_alt;
    if (!wpnav_get_vector_NEU(next_destination, &next_dest_neu, &next_dest_terr_alt)) {
        return false;
    }

    // set target as vector from EKF origin
    return wpnav_set_spline_destination(wpnav, &dest_neu, dest_terr_alt, &next_dest_neu, next_dest_terr_alt, next_is_spline);
}

/// set next destination (e.g. the one after the current destination) as a spline segment specified as a location
///     returns false if conversion from location to vector from ekf origin cannot be calculated
///     next_next_destination should be the next segment's destination
bool wpnav_set_spline_destination_next_loc(mc_wpnav* wpnav, const Location* next_destination, const Location* next_next_destination, bool next_next_is_spline)
{
    // convert next_destination location to vector
    Vector3f_t next_dest_neu = VECTOR3F_DEFAULT_VALUE;
    bool next_dest_terr_alt;
    if (!wpnav_get_vector_NEU(next_destination, &next_dest_neu, &next_dest_terr_alt)) {
        return false;
    }

    // convert next_next_destination to vector
    Vector3f_t next_next_dest_neu = VECTOR3F_DEFAULT_VALUE;
    bool next_next_dest_terr_alt;
    if (!wpnav_get_vector_NEU(next_next_destination, &next_next_dest_neu, &next_next_dest_terr_alt)) {
        return false;
    }

    // set target as vector from EKF origin
    return wpnav_set_spline_destination_next(wpnav, &next_dest_neu, next_dest_terr_alt, &next_next_dest_neu, next_next_dest_terr_alt, next_next_is_spline);
}

/// set_spline_destination waypoint using position vector (distance from ekf origin in cm)
///     terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin)
///     next_destination should be set to the next segment's destination
///     next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin)
///     next_destination.z  must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination should be too)
bool wpnav_set_spline_destination(mc_wpnav* wpnav, const Vector3f_t* destination, bool terrain_alt, const Vector3f_t* next_destination, bool next_terrain_alt, bool next_is_spline)
{
     Vector3f_t v3_zero = VECTOR3F_DEFAULT_VALUE;

    // re-initialise if previous destination has been interrupted
    if (!wpnav_is_active(wpnav) || !wpnav->_flags.reached_destination) {
        wpnav_wp_and_spline_init(wpnav, wpnav->_wp_desired_speed_xy_cms, v3_zero);
    }

    // update spline calculators speeds and accelerations
    slcurve_set_speed_accel(&wpnav->_spline_this_leg, posctrl_get_max_speed_xy_cms(wpnav->_pos_control), posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_speed_down_cms(wpnav->_pos_control),
                                     posctrl_get_max_accel_xy_cmss(wpnav->_pos_control), posctrl_get_max_accel_z_cmss(wpnav->_pos_control));

    // calculate origin and origin velocity vector
    Vector3f_t origin_vector = VECTOR3F_DEFAULT_VALUE;
    if (terrain_alt == wpnav->_terrain_alt) {
        if (wpnav->_flags.fast_waypoint) {
            // calculate origin vector
            if (wpnav->_this_leg_is_spline) {
                // if previous leg was a spline we can use destination velocity vector for origin velocity vector
                origin_vector = slcurve_get_destination_vel(&wpnav->_spline_this_leg);
            } else {
                // use direction of the previous straight line segment
                origin_vector.x = wpnav->_destination.x - wpnav->_origin.x;
                origin_vector.y = wpnav->_destination.y - wpnav->_origin.y;
                origin_vector.z = wpnav->_destination.z - wpnav->_origin.z;
            }
        }

        // use previous destination as origin
        wpnav->_origin = wpnav->_destination;
    } else {

        // use previous destination as origin
        wpnav->_origin = wpnav->_destination;

        // get current alt above terrain
        float origin_terr_offset;
        if (!wpnav_get_terrain_offset(wpnav, &origin_terr_offset)) {
            return false;
        }

        // convert origin to alt-above-terrain if necessary
        if (terrain_alt) {
            // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
            wpnav->_origin.z -= origin_terr_offset;
            posctrl_set_pos_offset_z_cm(wpnav->_pos_control, posctrl_get_pos_offset_z_cm(wpnav->_pos_control) + origin_terr_offset);
        } else {
            // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
            wpnav->_origin.z += origin_terr_offset;
            posctrl_set_pos_offset_z_cm(wpnav->_pos_control, posctrl_get_pos_offset_z_cm(wpnav->_pos_control) - origin_terr_offset);
        }
    }

    // store destination location
    wpnav->_destination = *destination;
    wpnav->_terrain_alt = terrain_alt;

    // calculate destination velocity vector
    Vector3f_t destination_vector = VECTOR3F_DEFAULT_VALUE;
    if (terrain_alt == next_terrain_alt) {
        if (next_is_spline) {
            // leave this segment moving parallel to vector from origin to next destination
            destination_vector.x = next_destination->x - wpnav->_origin.x;
            destination_vector.y = next_destination->y - wpnav->_origin.y;
            destination_vector.z = next_destination->z - wpnav->_origin.z;
        } else {
            // leave this segment moving parallel to next segment
            destination_vector.x = next_destination->x - wpnav->_destination.x;
            destination_vector.y = next_destination->y - wpnav->_destination.y;
            destination_vector.z = next_destination->z - wpnav->_destination.z;
        }
    }
    wpnav->_flags.fast_waypoint = !vec3_is_zero(&destination_vector);

    // setup spline leg
    slcurve_set_origin_and_destination(&wpnav->_spline_this_leg, &wpnav->_origin, &wpnav->_destination, &origin_vector, &destination_vector);
    wpnav->_this_leg_is_spline = true;
    wpnav->_flags.reached_destination = false;

    return true;
}

/// set next destination (e.g. the one after the current destination) as an offset (in cm, NEU frame) from the EKF origin
///     next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin)
///     next_next_destination should be set to the next segment's destination
///     next_next_terrain_alt should be true if next_next_destination.z is a desired altitude above terrain (false if it is desired altitude above ekf origin)
///     next_next_destination.z  must be in the same "frame" as destination.z (i.e. if next_destination is a alt-above-terrain, next_next_destination should be too)
bool wpnav_set_spline_destination_next(mc_wpnav* wpnav, const Vector3f_t* next_destination, bool next_terrain_alt, const Vector3f_t* next_next_destination, bool next_next_terrain_alt, bool next_next_is_spline)
{
    // do not add next point if alt types don't match
    if (next_terrain_alt != wpnav->_terrain_alt) {
        return true;
    }

    // calculate origin and origin velocity vector
    Vector3f_t origin_vector = VECTOR3F_DEFAULT_VALUE;
    if (wpnav->_this_leg_is_spline) {
        // if previous leg was a spline we can use destination velocity vector for origin velocity vector
        origin_vector = slcurve_get_destination_vel(&wpnav->_spline_this_leg);
    } else {
        // use the direction of the previous straight line segment
        origin_vector.x = wpnav->_destination.x - wpnav->_origin.x;
        origin_vector.y = wpnav->_destination.y - wpnav->_origin.y;
        origin_vector.z = wpnav->_destination.z - wpnav->_origin.z;
    }

    // calculate destination velocity vector
    Vector3f_t destination_vector = VECTOR3F_DEFAULT_VALUE;
    if (next_terrain_alt == next_next_terrain_alt) {
        if (next_next_is_spline) {
            // leave this segment moving parallel to vector from this leg's origin (i.e. prev leg's destination) to next next destination
            destination_vector.x = next_next_destination->x - wpnav->_destination.x;
            destination_vector.y = next_next_destination->y - wpnav->_destination.y;
            destination_vector.z = next_next_destination->z - wpnav->_destination.z;
        } else {
            // leave this segment moving parallel to next segment
            destination_vector.x = next_next_destination->x - next_destination->x;
            destination_vector.y = next_next_destination->y - next_destination->y;
            destination_vector.z = next_next_destination->z - next_destination->z;
        }
    }

    // update spline calculators speeds and accelerations
    slcurve_set_speed_accel(&wpnav->_spline_next_leg, posctrl_get_max_speed_xy_cms(wpnav->_pos_control), posctrl_get_max_speed_up_cms(wpnav->_pos_control), posctrl_get_max_speed_down_cms(wpnav->_pos_control),
                                     posctrl_get_max_accel_xy_cmss(wpnav->_pos_control), posctrl_get_max_accel_z_cmss(wpnav->_pos_control));

    // setup next spline leg.  Note this could be made local
    slcurve_set_origin_and_destination(&wpnav->_spline_next_leg, &wpnav->_destination, next_destination, &origin_vector, &destination_vector);
    wpnav->_next_leg_is_spline = true;

    // next destination provided so fast waypoint
    wpnav->_flags.fast_waypoint = true;

    // update this_leg's final velocity to match next spline leg
    if (!wpnav->_this_leg_is_spline) {
        scurve_set_destination_speed_max(&wpnav->_scurve_this_leg, slcurve_get_origin_speed_max(&wpnav->_spline_next_leg));
    } else {
        slcurve_set_destination_speed_max(&wpnav->_spline_this_leg, slcurve_get_origin_speed_max(&wpnav->_spline_next_leg));
    }

    return true;
}

/**
  * @brief       
  * @param[in]   wpnav  
  * @param[out]  
  * @retval      
  * @note        
  */
static void assign_param(mc_wpnav* wpnav)
{
    param_link_variable(PARAM_ID(WPNAV, WPNAV_SPEED), &wpnav->_wp_speed_cms);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_RADIUS), &wpnav->_wp_radius_cm);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_SPEED_UP), &wpnav->_wp_speed_up_cms);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_SPEED_DN), &wpnav->_wp_speed_down_cms);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_ACCEL), &wpnav->_wp_accel_cmss);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_ACCEL_Z), &wpnav->_wp_accel_z_cmss);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_RFND_USE), &wpnav->_rangefinder_use);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_JERK), &wpnav->_wp_jerk);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_TER_MARGIN), &wpnav->_terrain_margin);
}

static void wpnav_param_set_and_save_ifchanged(const char* param_name, double value)
{
    param_t* p = NULL;

    for(int i= 0; i < ARRAY_SIZE(var_info); i++){
        if(strcmp(param_name, var_info[i].name) == 0) {
            p = &var_info[i];
            break;
        }
    }

    if (p != NULL) {
        param_set_and_save_ifchanged(p, value);
    }
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    else {
        printf("[wpnav]: The parameter to be set does not exist!\n");
    }
#endif
}

// helper function to calculate scurve jerk and jerk_time values
// updates _scurve_jerk and _scurve_snap
static void wpnav_calc_scurve_jerk_and_snap(mc_wpnav* wpnav)
{
    // calculate jerk
    wpnav->_scurve_jerk = MIN(attctrl_get_ang_vel_roll_max_rads(wpnav->_attitude_control) * GRAVITY_MSS, attctrl_get_ang_vel_pitch_max_rads(wpnav->_attitude_control) * GRAVITY_MSS);
    if (math_flt_zero(wpnav->_scurve_jerk)) {
        wpnav->_scurve_jerk = wpnav->_wp_jerk;
    } else {
        wpnav->_scurve_jerk = MIN(wpnav->_scurve_jerk, wpnav->_wp_jerk);
    }

    // calculate maximum snap
    // Snap (the rate of change of jerk) uses the attitude control input time constant because multicopters
    // lean to accelerate. This means the change in angle is equivalent to the change in acceleration
    wpnav->_scurve_snap = (wpnav->_scurve_jerk * M_PI) / (2.0f * MAX(attctrl_get_input_tc(wpnav->_attitude_control), 0.1f));
    const float snap = MIN(attctrl_get_accel_roll_max_radss(wpnav->_attitude_control), attctrl_get_accel_pitch_max_radss(wpnav->_attitude_control)) * GRAVITY_MSS;
    if (math_flt_positive(snap)) {
        wpnav->_scurve_snap = MIN(wpnav->_scurve_snap, snap);
    }
    // reduce maximum snap by a factor of two from what the aircraft is capable of
    wpnav->_scurve_snap *= 0.5f;
}

/*------------------------------------test------------------------------------*/


